/*
 * @Description: 数据预处理模块，包括导入话题名，时间同步，和去畸变。这里暂时没做时间同步和去畸变
 * @Author: Sang Hao
 * @Date: 2021-10-26 11:36:23
 * @LastEditTime: 2021-11-22 21:33:04
 * @LastEditors: Sang Hao
 */

#ifndef LIDAR_SLAM_DATA_PRETREAT_DATA_PRETREAT_FLOW_HPP_
#define LIDAR_SLAM_DATA_PRETREAT_DATA_PRETREAT_FLOW_HPP_

#include <ros/ros.h>
// subscriber
#include "lidar_slam/subscriber/cloud_subscriber.hpp"
#include "lidar_slam/subscriber/velocity_subscriber.hpp"
#include "lidar_slam/tf_listener/tf_listener.hpp"
// publisher
#include "lidar_slam/publisher/cloud_publisher.hpp"
#include "lidar_slam/publisher/odometry_publisher.hpp"
// models
#include "lidar_slam/models/scan_adjust/distortion_adjust.hpp"
#include "lidar_slam/models/cloud_filter/cloud_filter_interface.hpp"
#include "lidar_slam/models/extract/plane_ransac_extract.hpp"

namespace lidar_slam {
class DataPretreatFlow {
private:
    // subscriber
    std::shared_ptr<CloudSubscriber> cloud_sub_ptr_;
    std::shared_ptr<VelocitySubscriber> velocity_sub_ptr_;
    std::shared_ptr<TFListener> lidar_to_imu_ptr_;
    // publisher
    std::shared_ptr<CloudPublisher> cloud_pub_ptr_;
    // models
    std::shared_ptr<DistortionAdjust> distortion_adjust_ptr_;
    std::shared_ptr<PlaneRansacExtract> vertical_plane_extract_;

    std::shared_ptr<CloudFilterInterface> range_filter_ptr_;
    std::shared_ptr<CloudFilterInterface> outlier_filter_ptr_;
    std::shared_ptr<CloudFilterInterface> multi_plane_clipper_filter_ptr_;/* 多平面裁剪器 */

    /* 给两个滤波加一个flag，这样的话距离其实也可以做一个滤波类 */

    Eigen::Matrix4f lidar_to_imu_ = Eigen::Matrix4f::Identity();

    std::deque<CloudData> cloud_data_buff_;
    std::deque<VelocityData> velocity_data_buff_;

    CloudData current_cloud_data_;
    VelocityData current_velocity_data_;

    /* 点云话题名 */
    std::string topic_name_ = "";
    
    bool use_range_filter_ = false;
    bool use_outlier_filter_ = false;
    bool use_multi_plane_clipper_filter_ = false;
private:
    bool ReadData();
    bool InitCalibration();
    bool InitWithConfig();
    bool InitFilter(const YAML::Node& config_node);
    bool HasData();
    bool ValidData();
    bool TransformData();
    bool PublishData();
    bool FilterData();

public:

    DataPretreatFlow(ros::NodeHandle& nh);
    bool Run();
};
}

#endif